package ai.agents.braitenberg;

import java.util.ArrayList;

import ai.agents.Agent;
import ai.application.Config;
import ai.application.Environment;
import ai.lights.Lightable;
import ai.spatial.Vector;

public abstract class BraitenbergAgent implements Agent {

	protected Vector desiredVelocity;
	protected Vector currentVelocity;
	protected Vector position;

	public BraitenbergAgent(Vector desiredVelocity, Vector currentVelocity) {
		this.desiredVelocity = desiredVelocity;
		this.currentVelocity = currentVelocity;
	}
	
	protected Vector adjustDesiredVelocity(Vector v1){
		double velocityAdjuster = 0.5;
		return new Vector(velocityAdjuster*v1.getX()*(Config.LIGHT_SENSIBILITY-v1.length()), velocityAdjuster*v1.getY()*(Config.LIGHT_SENSIBILITY-v1.length()));
	}
	
	protected Vector calculateDesiredLightVelocity(ArrayList<Lightable> lights){
		this.desiredVelocity = new Vector(0,0);
		Vector newDesiredVelocity = new Vector(0,0);
		for (Lightable light : lights) {
			if(!light.equals(this)){
				Vector vectorVehicleLight = light.getPosition().substract(this.position);
				double angle = this.currentVelocity.angle(vectorVehicleLight);
				if(angle < 90){
					double distanceToLight = vectorVehicleLight.length();
					if(distanceToLight < Config.LIGHT_SENSIBILITY){
						Vector adjustedVelocity = this.adjustDesiredVelocity(vectorVehicleLight);
						newDesiredVelocity = newDesiredVelocity.add(adjustedVelocity);
					}
				}
			}
		}
		return newDesiredVelocity;
	}
	
	protected Vector getBorderForces(Environment environment){
		
		Vector desiredBorderVelocity = new Vector(0,0);
		
		double distanceXMin = this.position.getX() - environment.getMinX(); 
		double distanceYMin = this.position.getY() - environment.getMinY(); 
		double distanceXMax = environment.getMaxX() - this.position.getX(); 
		double distanceYMax = environment.getMaxY() - this.position.getY();
		
		if(distanceXMin < Config.BORDER_SENSIBILITY){
			desiredBorderVelocity = desiredBorderVelocity.add(new Vector(Config.BORDER_SENSIBILITY-distanceXMin, 0));
		}
		if(distanceYMin < Config.BORDER_SENSIBILITY){
			desiredBorderVelocity = desiredBorderVelocity.add(new Vector(0, Config.BORDER_SENSIBILITY-distanceYMin));
		}
		if(distanceXMax < Config.BORDER_SENSIBILITY){
			desiredBorderVelocity = desiredBorderVelocity.add(new Vector(-(Config.BORDER_SENSIBILITY-distanceXMax), 0));
		}
		if(distanceYMax < Config.BORDER_SENSIBILITY){
			desiredBorderVelocity = desiredBorderVelocity.add(new Vector(0,-(Config.BORDER_SENSIBILITY - distanceYMax)));
		}
		return desiredBorderVelocity.multiply(0.8);
//		System.out.println("x: "+desiredBorderVelocity.getX()+"y: "+ desiredBorderVelocity.getY());
//		return new Vector(desiredBorderVelocity.getX() > 5 ? 5: desiredBorderVelocity.getX(), 
//				desiredBorderVelocity.getY() > 5 ? 5 : desiredBorderVelocity.getY());
		
	}

	@Override
	public Vector getDesiredVelocity() {
		return desiredVelocity;
	}
	
	@Override
	public Vector getCurrentVelocity() {
		return currentVelocity;
	}

	public Vector getPosition() {
		return position;
	}

	public void setPosition(Vector position) {
		this.position = position;
	}
	
}
